import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory #获取功能包的资源路径
import os
from launch.substitutions import FindExecutable, PathJoinSubstitution

import launch_ros.parameter_descriptions

def generate_launch_description():   #这个函数名是固定的，不可修改
    """产生launch描述"""
    # # 获取URDF文件路径
    # urdf_file = PathJoinSubstitution([
    #     get_package_share_directory('fishbot_description'),
    #     'urdf',
    #     'first_robot.urdf'
    # ])



    # # 读取URDF内容
    # robot_description_content = launch.substitutions.Command([
    #     FindExecutable(name='cat'), ' ', urdf_file
    # ])

    # 获取xacro文件路径
    xacro_file = PathJoinSubstitution([
        get_package_share_directory('fishbot_description'),
        'urdf','fishbot',
        'fishbot.urdf.xacro'
    ])


    # 读取xacro内容
    robot_description_content = launch.substitutions.Command([
        FindExecutable(name='xacro'), ' ', xacro_file              #相当于在命令行 xacro 文件绝对路径生成urdf文件
    ])
    #将 URDF/XACRO 文件内容 转换为一个 ROS 2 参数值（ParameterValue），以便后续传递给节点
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        robot_description_content,
        value_type=str
    )

    
    #获取rviz文件路径
    rviz_file = PathJoinSubstitution([
        get_package_share_directory('fishbot_description'),
        'rviz',
        'display_robot_model.rviz'
    ])

    #启动节点
    #订阅joint，结合 URDF 中的运动学链，计算每个连杆的坐标系
    action_robot_state_publisher = launch_ros.actions.Node(
        package='robot_state_publisher',         #功能包名字
        executable='robot_state_publisher', #可执行文件名
        parameters=[{'robot_description':robot_description}]
    )
    #发布静态TF关节节点
    action_joint_state_publisher = launch_ros.actions.Node(
        package='joint_state_publisher',         #功能包名字
        executable='joint_state_publisher', #可执行文件名

    )

    #打开rviz,并加载配置文件
    action_rviz_node = launch_ros.actions.Node(
        package='rviz2',         #功能包名字
        executable='rviz2', #可执行文件名
        arguments=['-d',rviz_file]

    )


    return launch.LaunchDescription([
        #actions动作
        action_robot_state_publisher,
        action_joint_state_publisher,
        action_rviz_node

    ])